#include "ros_modbus.h"
#include"CRC16.h"
#include"config.h"
using namespace std;
using namespace boost::asio;
//串口相关对象
boost::asio::io_service iosev;
boost::asio::serial_port sp(iosev, "/dev/ttyUSB0");
boost::system::error_code err;
/********************************************************
            串口发送接收相关常量、变量、共用体对象
********************************************************/
MODBUS modbus;
const unsigned char ender[2] = {0x0d, 0x0a};
const unsigned char header[2] = {0x55, 0xaa};
extern unsigned char  write_CHASSIS0[8];
extern unsigned char  write_CHASSIS1[8];
//发送左右轮速控制速度共用体
union sendData
{
	short d;
	unsigned char data[2];
}leftVelSet,rightVelSet;

//接收数据（左轮速、右轮速、角度）共用体（-32767 - +32768）
union receiveData
{
	short d;
	unsigned char data[2];
}leftVelNow,rightVelNow,angleNow;

/**
  * @brief          串口参数初始化
  * @param[in]      none
  * @retval         none
  */
void serialInit()
{
    sp.set_option(serial_port::baud_rate(115200));
    sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
    sp.set_option(serial_port::parity(serial_port::parity::none));
    sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
    sp.set_option(serial_port::character_size(8));    
}

/**
  * @brief         将Modbus消息发送至下位机
  * @param[in]     *buf:需要发送数据的指针
  * @retval         none
  */
 void writeModbus_in(unsigned char addr,unsigned int reg,unsigned int value)
 {
  //   unsigned char buf[] ;
    unsigned char cont = 0;
    unsigned int crc;
     modbus.Sendbuf[cont++] = addr;
     modbus.Sendbuf[cont++] = 0x06; //写
     modbus.Sendbuf[cont++] = reg /256;     //寄存器高8位
     modbus.Sendbuf[cont++] = reg%256;
    modbus.Sendbuf[cont++] = value /256;
    modbus.Sendbuf[cont++] = value%256;
    crc = crc16(modbus.Sendbuf,cont);
    modbus.Sendbuf[cont++] = crc/256;  
    modbus.Sendbuf[cont++] = crc%256;
    boost::asio::write(sp, boost::asio::buffer(modbus.Sendbuf));
    ROS_INFO("transfer_ok!");
 }
void writeModbus()
{    
    // 通过串口下发数据
    //unsigned char bufs[] = write_CHASSIS0;
    boost::asio::write(sp, boost::asio::buffer(write_CHASSIS0));
    ROS_INFO("OK!!!");
}
void writeModbus2()
{    
    // 通过串口下发数据
    //unsigned char bufs[] = write_CHASSIS0;
    boost::asio::write(sp, boost::asio::buffer(write_CHASSIS1));
    ROS_INFO("OK！");
}

/**
  * @brief         从下位机读取数据
  * @param[in]     none
  * @retval        none
  */
 /*
bool read_modbus(void)
{
    char i, length = 0;
    unsigned char checkSum;
    unsigned char buf[150]={0};
    //=========================================================
    //此段代码可以读数据的结尾，进而来进行读取数据的头部
    try
    {
        boost::asio::streambuf response;
        boost::asio::read_until(sp, response, "\r\n",err);   
        copy(istream_iterator<unsigned char>(istream(&response)>>noskipws),
        istream_iterator<unsigned char>(),
        buf); 
    }  
    catch(boost::system::system_error &err)
    {
        ROS_INFO("read_until error");
    } 
    //=========================================================        

    // 检查信息头
    unsigned int  crc;
    unsigned char crch, crcl;

    if ((buf[0]!=0x01)&&(buf[0]!=0x02)&&(buf[0]!=0x08)) //不是从机地址不处理
    {                   
        return;         
    } 
    //地址相符时，再对本帧数据进行校验
    crc = GetCRC16(buf, len-2);  //计算CRC校验值
    crch = crc >> 8;
    crcl = crc & 0xFF;
    if ((buf[len-2]!=crch) || (buf[len-1]!=crcl))
    {
        return;   //如CRC校验不符时直接退出
    }
    //地址和校验字均相符后，解析功能码，执行相关操作
    switch (buf[1])//shujubuf[4];//数据缓存 0存温度,1存湿度,2存LED,3存REL
    {
        case 0x03:  //对从机的03回复数据进行处理
            		if (buf[2]==0x02) //数据2字节
						{  
         				if(buf[0]==0x02)//温度从机地址
							{
								shujubuf[0]=buf[4];//数据缓存 0存温度
							}
							else if(buf[0]==0x08)//湿度从机地址
							{
								shujubuf[1]=buf[4];//数据缓存1存湿度
							}
						}
						else
                  {
							return;//指令不对退出
						}
            
        case 0x06:  //对从机的06回复数据进行处理//shujubuf[4];//数据缓存
            		if ((buf[2]==0x20) && (buf[3]==0x00))//LED数据地址
					{  
 						shujubuf[2]=buf[5];//把LED的状态存入数组
					}

					else if((buf[2]==0x20) && (buf[3]==0x01))//REL数据地址
					{
						shujubuf[3]=buf[5];//把REL的状态存入数组
					}
	
            
        	 default:  //其它不支持的功能码
            			buf[1] |= 0x80;  //功能码最高位置1
            			buf[2] = 0x01;   //设置异常码为01-无效功能
            			len = 3;
            			break;
    }
    return true;
}
*/
/**
  * @brief          获得8位循环冗余校验值
  * @param[in]     ptr:数组地址
  * @param[in]     len:长度
  * @retval         校验值
  */
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
    unsigned char crc;
    unsigned char i;
    crc = 0;
    while(len--)
    {
        crc ^= *ptr++;
        for(i = 0; i < 8; i++)
        {
            if(crc&0x01)
                crc=(crc>>1)^0x8C;
            else 
                crc >>= 1;
        }
    }
    return crc;
}
